/*
 * image_process.c
 *
 *  Created on: Dec 11, 2023
 *      Author: 86152
 */
#include "zf_common_headfile.h"

#define AT                  AT_IMAGE
#define AT_CLIP(img, x, y)  AT_IMAGE((img), clip((x), 0, (img)->width-1), clip((y), 0, (img)->height-1));


//-------------------------------------------------------------------------------------------------------------------
uint8_t threshold;//大津法阈值

float aim_distance;

int drawline0[2];//补线起始点坐标
int drawline1[2];//补线终点坐标

float N_drawline0[2];//补线起始点坐标
float N_drawline1[2];//补线终点坐标

//逆透视补线数组
float left_line[POINTS_MAX_LEN][2];
float right_line[POINTS_MAX_LEN][2];
int left_num, right_num;

//拼接数组
float splicing_leftline[POINTS_MAX_LEN][2];
float splicing_rightline[POINTS_MAX_LEN][2];
int splicing_leftline_num,splicing_rightline_num;

//拼接数组平移中线
float splicing_leftline_center[POINTS_MAX_LEN][2];
float splicing_rightline_center[POINTS_MAX_LEN][2];
int splicing_leftline_center_num,splicing_rightline_center_num;

uint8  Image_Use[UROW][UCOL];             //灰度图像(60*80)
uint8  Image_Use_Robert[UROW][UCOL];      //二值化图像

image_t Image_Struct=
        {
                Image_Use[UROW],
                UROW,
                UCOL,
                0
        };

image_t img_raw = DEF_IMAGE(NULL, UCOL, UROW);

//左右边丢线
uint8 loseline0;
uint8 loseline1;

uint8 touch_boundary0;              //左边线走到图像左边界
uint8 touch_boundary1;              //右边线走到图像右边界

uint8 touch_boundary_up0;              //左边线走到图像上边界
uint8 touch_boundary_up1;              //右边线走到图像上边界

int begin_x0,begin_y0;              //找线偏移点
int begin_x1,begin_y1;              //找线偏移点

float xielv_left_y_to_end,xielv_right_y_to_end;                 //在逆透视后得坐标系建得斜率
//初始边线数据
int ipts0[POINTS_MAX_LEN][2];//存放边线数据（左）
int ipts1[POINTS_MAX_LEN][2];//存放边线数据（右）
int ipts0_num;//存放边线像素点个数(左)
int ipts1_num;//存放边线像素点个数(右)
// 变换后左右边线
float rpts0[POINTS_MAX_LEN][2];
float rpts1[POINTS_MAX_LEN][2];
int rpts0_num, rpts1_num;
// 变换后左右边线+滤波
float rpts0b[POINTS_MAX_LEN][2];
float rpts1b[POINTS_MAX_LEN][2];
int rpts0b_num, rpts1b_num;
// 变换后左右边线+等距采样
float rpts0s[POINTS_MAX_LEN][2];
float rpts1s[POINTS_MAX_LEN][2];
int rpts0s_num, rpts1s_num;
// 左右边线局部角度变化率
float rpts0a[POINTS_MAX_LEN];
float rpts1a[POINTS_MAX_LEN];
int rpts0a_num, rpts1a_num;
// 左右边线局部角度变化率+非极大抑制
float rpts0an[POINTS_MAX_LEN];
float rpts1an[POINTS_MAX_LEN];
int rpts0an_num, rpts1an_num;
// 左/右中线
float rptsc0[POINTS_MAX_LEN][2];
float rptsc1[POINTS_MAX_LEN][2];
int rptsc0_num, rptsc1_num;
// 左右跟踪后的中线
float (*rpts)[2];
int rpts_num;
// 归一化最终中线
float rptsn[POINTS_MAX_LEN][2];
int rptsn_num;

// Y角点
int Ypt0_rpts0s_id, Ypt1_rpts1s_id;
bool Ypt0_found, Ypt1_found;
// L角点
int Lpt0_rpts0s_id, Lpt1_rpts1s_id;
bool Lpt0_found, Lpt1_found;
// 内L角点
int N_Lpt0_rpts0s_id, N_Lpt1_rpts1s_id;
bool N_Lpt0_found, N_Lpt1_found;
// 长直道
bool is_straight0, is_straight1;
// 弯道
bool is_turn0, is_turn1;

int N_Xfound_num;//面向赛道编程，双内L计数

// 当前巡线模式（发车方向）
enum track_type_e track_type = TRACK_RIGHT;

float error[1];
float ave_error;//速度控制输入变量
// 多个预锚点位置
int aim_idx[1] ;


// 计算远锚点偏差值
float dx[1] ;
float dy[1] ;
float dn[1] ;


// 若考虑近点远点,可近似构造Stanley算法,避免撞路肩
// 计算近锚点偏差值
float dx_near;
float dy_near ;
float dn_near ;
float pure_angle;

//image_t img_change = DEF_IMAGE(NULL, RESULT_COL, RESULT_ROW);//定义逆透视后图像对象（暂时未赋值数据）
//uint8_t img_thres_data[RESULT_COL][RESULT_ROW];//二值化图像数据
//image_t img_thres = DEF_IMAGE((uint8_t *) img_thres_data, RESULT_COL, RESULT_ROW);
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     压缩图像
// 参数说明     无
// 返回参数     无
// 使用示例     compressimage(void);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void compressimage(void)
{
    int i, j, row, line;
    const float div_h = MT9V03X_H / UROW, div_w = MT9V03X_W / UCOL;
    for (i = 0; i < UROW; i++)
    {
        row = i * div_h + 0.5;
        for (j = 0; j < UCOL; j++)
        {
            line = j * div_w + 0.5;
            Image_Use[i][j] = mt9v03x_image[row][line];
        }
    }
    mt9v03x_finish_flag = 0;  //使用完一帧DMA传输的图像图像  可以开始传输下一帧
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     二值化
// 参数说明     thro （阈值）
// 返回参数     无
// 使用示例     Binarization(uint8 thro);
// 备注信息     内部已申明的图像数组的二值化
//-------------------------------------------------------------------------------------------------------------------
void Binarization(uint8 thro)
{
    uint32 i,j;
    uint16 temp = 0;
    for(j=0;j<UROW;j++)
    {
        for(i=0;i<UCOL;i++)
        {
            temp = *(Image_Use[0]+j*UCOL+i);//读取像素点
            if(j == 0 || j == UROW-1 || i == 0 || i == UCOL-1)//大津法加一个黑框
            {
                *(Image_Use_Robert[0]+j*UCOL+i)=0;
            }
            else
            {
                if(temp>=thro) *(Image_Use_Robert[0]+j*UCOL+i)=255;
                else *(Image_Use_Robert[0]+j*UCOL+i)=0;
            }
        }
    }
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     自适应二值化
// 参数说明     uint8_t类型的指针img_data（指向输入图像数据）
//             uint8_t类型的指针output_data（指向输出图像数据）
//             int类型的变量width和height（分别表示输入图像的宽度和高度）
//             int类型的变量block（表示用于计算阈值的局部图像区域的尺寸）
//             uint8_t类型的变量clip_value（表示剪切值）
// 返回参数     无
// 使用示例     adaptiveThreshold(img_data[], output_data[], width, height, block, clip_value);
// 备注信息     由此代码可见自适应二值化的计算量特别大
//-------------------------------------------------------------------------------------------------------------------
void adaptiveThreshold(uint8_t* img_data, uint8_t* output_data, int width, int height, int block, uint8_t clip_value)
{  
    // 断言语句，检查传入的block参数是否为奇数。如果block不是奇数，程序将终止执行并报告错误
    zf_assert(block % 2 == 1); // block必须为奇数
    // 计算block的一半（half_block），用于确定局部图像区域的边界
    int half_block = block / 2;
    // 外层循环，遍历图像的每一行像素。注意，由于我们计算的是每一行的中间像素到边界的距离，因此只需要遍历中间像素及其之间的像素即可
    for(int y=half_block; y<height-half_block; y++)
    {
        // 内层循环，遍历图像的每一列像素。同样地，我们只需要考虑中间像素及其之间的像素
        for(int x=half_block; x<width-half_block; x++)
        {
            // 初始化阈值变量thres为0，用于存储局部图像区域的像素值之和
            // 计算局部阈值
            int thres = 0;
            // 内嵌的两层循环，遍历以当前像素为中心的局部图像区域。对于每个像素，我们将它的值加到thres中
            for(int dy=-half_block; dy<=half_block; dy++)
            {
                for(int dx=-half_block; dx<=half_block; dx++)
                {
            thres += img_data[(x+dx)+(y+dy)*width];
                }
            }
            // 将局部图像区域的像素值之和除以局部区域的面积（即block*block），然后减去clip_value，得到阈值thres
            thres = thres / (block * block) - clip_value;
            // 进行二值化
            output_data[x+y*width] = img_data[x+y*width]>thres ? 255 : 0;
        }  
    }  
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     大津法求阈值
// 参数说明     image_t (图像结构体)
//          MinThreshold (最小阈值)
//          MaxThreshold (最大阈值)
// 返回参数     OUSTThreshold
// 使用示例     getOSTUThreshold(&img, MinThreshold, MaxThreshold);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------

uint16_t getOSTUThreshold(image_t *img, uint8_t MinThreshold, uint8_t MaxThreshold)
{
    uint8_t  Histogram[256];
    uint16_t OUSTThreshold = 0;
    uint32_t PixelAmount = 0, Value_Sum = 0;
    uint64_t sigma = 0, maxSigma = 0;
    float w1 = 0, w2 = 0;
    int32_t u1 = 0, u2 = 0;
    uint8_t MinValue = 0, MaxValue = 255;

    //各像素点个数
    uint8_t *ptr = img->data;
    uint8_t *ptrEnd = img->data + img->width * img->height;
    while (ptr != ptrEnd)
    {
        ++Histogram[*ptr++];
    }
    for (uint8_t m = 0; m < 100; m++)
    {
        Histogram[m] = 0;
    }

    for (MinValue = 0; Histogram[MinValue] == 0 && MinValue < 255; ++MinValue);
    for (MaxValue = 255; Histogram[MaxValue] == 0 && MaxValue > 0; --MaxValue);

    if (MaxValue == MinValue) return MaxValue;                      // 只有一个颜色
    if (MinValue + 1 == MaxValue) return MinValue;                  // 只有二个颜色

    if (MinValue < MinThreshold)
    {
        MinValue = MinThreshold;
    }
    if (MaxValue > MaxThreshold)
    {
        MaxValue = MaxThreshold;
    }

    uint32_t Pixel_Integral[256] = {0};                             //像素积分
    uint32_t Value_Integral[256] = {0};                             //灰度积分
    for (uint8_t i = MinValue; i <= MaxValue; ++i)
    {
        PixelAmount += Histogram[i];                                //像素总数
        Value_Sum += Histogram[i] * i;                              //灰度总和
        Pixel_Integral[i] = PixelAmount;
        Value_Integral[i] = Value_Sum;
    }
    for (uint8_t i = MinValue; i < MaxValue + 1; ++i)
    {
        w1 = (float) Pixel_Integral[i] / PixelAmount;               //前景像素点比例
        w2 = 1 - w1;                                                //背景比例
        u1 = (int32_t) (Value_Integral[i] / w1);                    //前景平均灰度
        u2 = (int32_t) ((Value_Sum - Value_Integral[i]) / w2);      //背景平均灰度
        sigma = (uint64_t) (w1 * w2 * (u1 - u2) * (u1 - u2));
        if (sigma >= maxSigma)
        {
            maxSigma = sigma;
            OUSTThreshold = i;
        }
        else
        {
            break;
        }
    }
    return OUSTThreshold;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     对原图像直接大津法求阈值
// 参数说明     void
// 返回参数     Threshold
// 使用示例     OSTU();
// 备注信息     这里是对逐飞库里原有存储像素的数组直接经行大津法求阈值
//-------------------------------------------------------------------------------------------------------------------
uint8 OSTU(void)
    {
    uint8 HistGram[256] = {0,};
    uint16 x,y;
    int16 Y;
    uint32 Amount = 0;
    uint32 PixelBack = 0;
    uint32 PixelIntegralBack = 0;
    uint32 PixelIntegral = 0;
    int32 PixelIntegralFore = 0;
    int32 PixelFore = 0;
    double OmegaBack, OmegaFore, MicroBack, MicroFore, SigmaB, Sigma; // 类间方差;
    int16 MinValue, MaxValue;
    uint8 Threshold = 0;

        for (y = 0; y < 256; y++)
        {
            HistGram[y] = 0; //初始化灰度直方图
        }
        for (y = 0; y < MT9V03X_H; y++)
        {
            for (x = 0; x < MT9V03X_W; x++)
            {
                HistGram[mt9v03x_image[y][x]]++; //统计每个灰度值的个数信息
            }
        }


    for (MinValue = 0; MinValue < 256 && HistGram[MinValue] == 0; MinValue++) ;        //获取最小灰度的值
    for (MaxValue = 255; MaxValue > MinValue && HistGram[MinValue] == 0; MaxValue--) ; //获取最大灰度的值

    if (MaxValue == MinValue)
    {
        return MaxValue;          // 图像中只有一个颜色
    }
    if (MinValue + 1 == MaxValue)
    {
        return MinValue;      // 图像中只有二个颜色
    }

    for (Y = MinValue; Y <= MaxValue; Y++)
    {
        Amount += HistGram[Y];        //  像素总数
    }

    PixelIntegral = 0;
    for (Y = MinValue; Y <= MaxValue; Y++)
    {
        PixelIntegral += HistGram[Y] * Y;//灰度值总数
    }
    SigmaB = -1;
    for (Y = MinValue; Y < MaxValue; Y++)
    {
        PixelBack = PixelBack + HistGram[Y];    //前景像素点数
        PixelFore = Amount - PixelBack;         //背景像素点数
        OmegaBack = (double)PixelBack / Amount;//前景像素百分比
        OmegaFore = (double)PixelFore / Amount;//背景像素百分比
        PixelIntegralBack += HistGram[Y] * Y;  //前景灰度值
        PixelIntegralFore = PixelIntegral - PixelIntegralBack;//背景灰度值
        MicroBack = (double)PixelIntegralBack / PixelBack;//前景灰度百分比
        MicroFore = (double)PixelIntegralFore / PixelFore;//背景灰度百分比
        Sigma = OmegaBack * OmegaFore * (MicroBack - MicroFore) * (MicroBack - MicroFore);//g
        if (Sigma > SigmaB)//遍历最大的类间方差g
        {
            SigmaB = Sigma;
            Threshold = Y;
        }
        if (Sigma < SigmaB)//遍历最大的类间方差g
        {
            break;
        }
    }
    return Threshold;
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     对原图像直接大津法求阈值
// 参数说明     void
// 返回参数     Threshold
// 使用示例     ostu();
// 备注信息     这里是对自己定义库里经行压缩过后存储像素的数组经行大津法求阈值
//-------------------------------------------------------------------------------------------------------------------
uint8 ostu(void)   //注意计算阈值的一定要是原图像
{
    #define GrayScale 256
    static uint8 lasthreshold=0;
    uint16 width = UCOL; //自己图像的宽度
    uint16 height = UROW; //自己图像的高度
    //uint16 width = MT9V03X_W;
    //uint16 height = MT9V03X_H;
    int pixelCount[GrayScale];
    float pixelPro[GrayScale];
    int i, j, pixelSum = width * height/4;
    uint8 threshold = 0;
    uint8* data = Image_Use[0];//uint8* data = mt9v03x_image[0];  //指向像素数据的指针
    for (i = 0; i < GrayScale; i++)
    {
        pixelCount[i] = 0;
        pixelPro[i] = 0;
    }

    uint32 gray_sum=0;
    //统计灰度级中每个像素在整幅图像中的个数
    for (i = 0; i < height; i+=2)
    {
        for (j = 0; j < width; j+=2)
        {
            pixelCount[(int)data[i * width + j]]++;  //将当前的点的像素值作为计数数组的下标
            gray_sum+=(int)data[i * width + j];       //灰度值总和
        }
    }

    //计算每个像素值的点在整幅图像中的比例

    for (i = 0; i < GrayScale; i++)
    {
        pixelPro[i] = (float)pixelCount[i] / pixelSum;

    }

    //遍历灰度级[0,255]
    float w0, w1, u0tmp, u1tmp, u0, u1, u, deltaTmp, deltaMax = 0;


        w0 = w1 = u0tmp = u1tmp = u0 = u1 = u = deltaTmp = 0;
        if( lasthreshold-50>0)
    j= lasthreshold-50;
        else
    j=0;
        for (; j < GrayScale; j++)
        {

                w0 += pixelPro[j];  //背景部分每个灰度值的像素点所占比例之和   即背景部分的比例
                u0tmp += j * pixelPro[j];  //背景部分 每个灰度值的点的比例 *灰度值

               w1=1-w0;
               u1tmp=gray_sum/pixelSum-u0tmp;

                u0 = u0tmp / w0;              //背景平均灰度
                u1 = u1tmp / w1;              //前景平均灰度
                u = u0tmp + u1tmp;            //全局平均灰度
                deltaTmp = w0 * (u0 - u) * (u0 - u) + w1 * (u1 - u) * (u1 - u);
                if (deltaTmp > deltaMax)
                {
                    deltaMax = deltaTmp;
                    threshold = j;
                }
                if (deltaTmp < deltaMax)
                {
                break;
                }

         }

    return threshold;
}
/************************************Sobel算子************************************
**          Gx={    {-1,  0,  1},                   Gy={    {  1,  2,  1},
**                  {-2,  0,  2},                           {  0,  0,  0},
**                  {-1,  0,  1}}                           { -1, -2, -1}}
**      最上下左右边缘舍去
**
**      Gx = (-1)*f(x-1, y-1) + 0*f(x,y-1) + 1*f(x+1,y-1)
**            +(-2)*f(x-1,y) + 0*f(x,y)+2*f(x+1,y)
**            +(-1)*f(x-1,y+1) + 0*f(x,y+1) + 1*f(x+1,y+1)
**      = [f(x+1,y-1)+2*f(x+1,y)+f(x+1,y+1)]-[f(x-1,y-1)+2*f(x-1,y)+f(x-1,y+1)]
**
**      Gy =1* f(x-1, y-1) + 2*f(x,y-1)+ 1*f(x+1,y-1)
**            +0*f(x-1,y) 0*f(x,y) + 0*f(x+1,y)
**            +(-1)*f(x-1,y+1) + (-2)*f(x,y+1) + (-1)*f(x+1, y+1)
**      = [f(x-1,y-1) + 2f(x,y-1) + f(x+1,y-1)]-[f(x-1, y+1) + 2*f(x,y+1)+f(x+1,y+1)]
**      若Threshold=0则输出灰度图      其他则二值化
***********************************************************************************/
#define Sobel_Gx(addr, y, x)    (addr[UP][RR]+2*addr[y][RR]+addr[DN][RR]-(addr[UP][LL]+2*addr[y][LL]+addr[DN][LL]))
#define Sobel_Gy(addr, y, x)    (addr[UP][LL]+2*addr[UP][x]+addr[UP][RR]-(addr[DN][LL]+2*addr[DN][x]+addr[DN][RR]))
#define Sobel_G(addr, y, x)     (abs(Sobel_Gx(addr, y, x)) + abs(Sobel_Gy(addr, y, x)))
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     S_Sobel算法二值化
// 参数说明     in_IMG（原图像）     out_IMG（处理过后的图像）   Threshold（阈值）
// 返回参数     void
// 使用示例     ostu();
// 备注信息     这里是对自己定义库里经行压缩过后存储像素的数组经行大津法求阈值
//-------------------------------------------------------------------------------------------------------------------
void S_Sobel(uint8 in_IMG[UROW][UCOL], uint8_t out_IMG[UROW][UCOL], uint16 Threshold)
{
    uint8_t i, j;
    uint8_t UP, DN, LL, RR;
    if (Threshold == 0)//观察每点梯度值
    {
        for (i = 1; i < UROW-1; i++)
        {
            DN = i + 1;     UP = i - 1;
            for (j = 1; j < UCOL-1; j++)
            {
                RR = j + 1;     LL = j - 1;
                out_IMG[i][j] = Sobel_G(in_IMG, i, j);
            }
        }
    }
    else//根据梯度值二值化
    {
        for (i = 1; i <  UROW-1; i++)
        {
            DN = i + 1;     UP = i - 1;
            for (j = 1; j < UCOL-1; j++)
            {
                RR = j + 1;     LL = j - 1;
                out_IMG[i][j] = (Sobel_G(in_IMG, i, j) >= Threshold ? 0 : 255);                    //修改的tft二值化图库函数：黑0；白1
            }
        }
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     图像逻辑与
// 参数说明     image_t (图像结构体)
// 返回参数      无
// 使用示例     image_and(&img0, &img1, &img2);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void image_and(image_t *img0, image_t *img1, image_t *img2)
{
    zf_assert(img0 && img0->data);
    zf_assert(img1 && img1->data);
    zf_assert(img2 && img2->data);
    zf_assert(img0->width == img1->width && img0->height == img1->height);
    zf_assert(img0->width == img2->width && img0->height == img2->height);

    // 先遍历y后遍历x比较cache-friendly
    for (int y = 0; y < img0->height; y++)
    {
        for (int x = 0; x < img0->width; x++)
        {
            AT(img2, x, y) = (AT(img0, x, y) == 0 || AT(img1, x, y) == 0) ? 0 : 255;
        }
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     图像逻辑或
// 参数说明     image_t (图像结构体)
// 返回参数      无
// 使用示例     image_or(&img0, &img1, &img2);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void image_or(image_t *img0, image_t *img1, image_t *img2)
{
    zf_assert(img0 && img0->data);
    zf_assert(img1 && img1->data);
    zf_assert(img2 && img2->data);
    zf_assert(img0->width == img1->width && img0->height == img1->height);
    zf_assert(img0->width == img2->width && img0->height == img2->height);
    // 先遍历y后遍历x比较cache-friendly
    for (int y = 0; y < img0->height; y++)
    {
        for (int x = 0; x < img0->width; x++)
        {
            AT(img2, x, y) = (AT(img0, x, y) == 0 && AT(img1, x, y) == 0) ? 0 : 255;
        }
    }
}
//-------------------------------------------------------------------------------------------------------------------
// 函数简介     2x2最小池化
// 参数说明     image_t (图像结构体)
// 返回参数      无
// 使用示例     minpool2(&img0, &img1);
// 备注信息     赛道边界是黑色，最小池化可以较好保留赛道边界
//-------------------------------------------------------------------------------------------------------------------

void minpool2(image_t *img0, image_t *img1)
{
    zf_assert(img0 && img0->data);
    zf_assert(img1 && img1->data);
    zf_assert(img0->width / 2 == img1->width && img0->height / 2 == img1->height);
    zf_assert(img0 != img1 && img0->data != img1->data);

    uint8_t min_value;
    // 先遍历y后遍历x比较cache-friendly
    for (int y = 1; y < img0->height; y += 2) {
        for (int x = 1; x < img0->width; x += 2) {
            min_value = 255;
            if (AT(img0, x, y) < min_value) min_value = AT(img0, x, y);
            if (AT(img0, x - 1, y) < min_value) min_value = AT(img0, x - 1, y);
            if (AT(img0, x, y - 1) < min_value) min_value = AT(img0, x, y - 1);
            if (AT(img0, x - 1, y - 1) < min_value) min_value = AT(img0, x - 1, y - 1);
                    AT(img1, x / 2, y / 2) = min_value;
        }
    }
}

//-------------------------------------------------------------------------------------------------------------------
// 函数简介     图像降噪
// 参数说明     image_t (图像结构体)
//          kernel (卷积核)
// 返回参数      无
// 使用示例     blur(&img0, &img1, kernel);
// 备注信息
//-------------------------------------------------------------------------------------------------------------------
void blur(image_t *img0, image_t *img1, uint32_t kernel)
{
    zf_assert(img0 && img0->data);
    zf_assert(img1 && img1->data);
    zf_assert(img0->width == img1->width && img0->height == img1->height);
    zf_assert(img0 != img1 && img0->data != img1->data);

    // 先遍历y后遍历x比较cache-friendly
    for (int y = 1; y < img0->height - 1; y++)
    {
        for (int x = 1; x < img0->width - 1; x++)
        {

            AT(img1, x, y) = (1 * AT(img0, x - 1, y - 1) + 2 * AT(img0, x, y - 1) + 1 * AT(img0, x + 1, y - 1) +
                              2 * AT(img0, x - 1, y) + 4 * AT(img0, x, y) + 2 * AT(img0, x + 1, y) +
                              1 * AT(img0, x - 1, y + 1) + 2 * AT(img0, x, y + 1) + 1 * AT(img0, x + 1, y + 1)) / 16;
        }
    }
}

/*************************************************************************
 *  函数名称：Find_Borderline();
 *  功能说明：寻找边线
 *  参数说明：无
 *  函数返回：无
 *  修改时间：2023年5月10日
 *  备    注：https://zhuanlan.zhihu.com/p/391392970 CSDN搜索上交迷宫法 （针对15cm限高优化）
 *
 *************************************************************************/
int x0_first , y0_first , x1_first ,y1_first;
int x , y ;
int x2 , y2 ;
void Find_Borderline(void)
{
    //迷宫巡线是否走到左右边界
    touch_boundary0 = 0;            //清零
    touch_boundary1 = 0;

    //迷宫巡线是否走到上边界
    touch_boundary_up0 = 0;            //清零
    touch_boundary_up1 = 0;

    //底边扫线防止丢线 注意：由于sobel边缘检测特殊性（黑框），此丢线标志仅适用Ostu方案
    loseline0 = 0;
    loseline1 = 0;

    uint8 uthres = 1;
//    uint8 uthres = ostu();
  //寻左边线
  x = (int)img_raw.width / 2 - begin_x, y = (int)begin_y;

  //标记种子起始点(后续元素处理要用到)
  x0_first = x;    y0_first = y;

  ipts0_num = sizeof(ipts0) / sizeof(ipts0[0]);//求数组的长度
  //扫底下五行，寻找跳变点
  for (; y0_first > begin_y-5; y0_first--)
  {
      for (; x0_first > 0; x0_first--)
          if (AT_IMAGE(&img_raw, x0_first - 1, y0_first) < uthres)
              goto out1;
      x0_first = img_raw.width / 2 - begin_x;
  }
  loseline0 = 1;  //底边丢线
  out1:
  {
      if (AT_IMAGE(&img_raw, x0_first, y0_first) >= uthres)
        Left_Adaptive_Threshold(&img_raw, block_size, clip_value, x0_first, y0_first, ipts0, &ipts0_num);
      else ipts0_num = 0;
  }



  //寻右边线
  x2 = img_raw.width / 2 + begin_x, y2 = begin_y;

  //标记种子起始点(后续元素处理要用到)
  x1_first = x2;    y1_first = y2;

  ipts1_num = sizeof(ipts1) / sizeof(ipts1[0]);
  for (; y1_first > begin_y-5; y1_first--)
  {
      for (; x1_first < img_raw.width - 1; x1_first++)
          if (AT_IMAGE(&img_raw, x1_first + 1, y1_first) < uthres)
              goto out2;
      x1_first = img_raw.width / 2 + begin_x;
  }
  loseline1 = 1;  //底边丢线
  out2:
  {
      if (AT_IMAGE(&img_raw, x1_first, y1_first) >= uthres)
        Right_Adaptive_Threshold(&img_raw, block_size, clip_value, x1_first, y1_first, ipts1, &ipts1_num);
      else ipts1_num = 0;
  }



}

/*************************************************************************
 *  函数名称：void Left_Adaptive_Threshold();
 *  功能说明：自定义阈值二值化+迷宫算法（左边线）
 *  参数说明：获取的图像数组，输出数组（备用image0），图像宽，图像高
 *  函数返回：无
 *  修改时间：2023年5月23日
 *  备    注： 增加丢线判断
 * 前进方向定义：
 *   0
 * 3   1
 *   2
 *************************************************************************/

const int dir_front[4][2] = { {0, -1},
                              {1,  0},
                              {0,  1},
                              {-1, 0}};
const int dir_frontleft[4][2] = {{-1, -1},
                                  {1,  -1},
                                   {1,  1},
                                  {-1, 1}};
const int dir_frontright[4][2] = {{1,  -1},
                                   {1,  1},
                                   {-1, 1},
                                  {-1, -1}};

void Left_Adaptive_Threshold(image_t *img,int block_size,int clip_value,int x,int y,int pts[][2],int *num)
{
    zf_assert(img && img->data);     // 不满足则退出执行
    zf_assert(num && *num >= 0);
    zf_assert(block_size > 1 && block_size % 2 == 1);//保证block_size为奇数
//    int half = block_size / 2;                       //上交方案
    int half = 0;                                  //方案二
    int step = 0, dir = 0, turn = 0; // step表示前进的步数；dir通过改变索引改变当前小人朝向的方向
    while (step < *num && half < x && x < img->width - half - 1 && half < y && y < img->height - half - 1 && turn < 4)//保证block不出界
    {
        int local_thres = 1;
//        int local_thres;
        //自适应二值化
    /*
        for (int dy = -half; dy <= half; dy++)  // for循环用来计算block区域的像素值之和（自适应阈值）
        {
            for (int dx = -half; dx <= half; dx++)
            {
                local_thres += AT(img, x + dx, y + dy);
            }
        }
        local_thres /= block_size * block_size;
        local_thres -= clip_value;   // (x,y)点block区域内的阈值
    */
        //int current_value = AT(img, x, y);//当前像素点灰度值
        int front_value = AT(img, x + dir_front[dir][0], y + dir_front[dir][1]);//正前方像素点灰度值 （dir=0 下；dir=1 右；dir=2 上；dir=3 左）
        int frontleft_value = AT(img, x + dir_frontleft[dir][0], y + dir_frontleft[dir][1]);//左前方像素点灰度值 （dir=0左下；dir=1 右下；dir=2 右上；dir=3 左上 ）
        //=======添加部分=======
        if( (x==1 && y < img->height -20) || x== img->width - 2 || y==1 || (y==58 && step > 19))
        {
            if(x==1 /*|| x== img->width - 2*/)  touch_boundary0  = 1;                //左边界是因为到最左边才停下来的，触碰到最左边，可能是环岛，十字等，
            if(y==1) touch_boundary_up0 = 1;               //走到顶边，判断坡道or障碍

            break;
        }
        //=======添加部分=======
        if (front_value < local_thres)  // 前进方向像素为黑色
        {
            dir = (dir + 1) % 4;   // 遇到前方为黑色需要右转一次
            turn++;
        }
        else if (frontleft_value < local_thres) // 前方像素为白色，且左前方像素为黑色
        {
            x += dir_front[dir][0];
            y += dir_front[dir][1];
            pts[step][0] = x;   // 用来存放边线坐标信息
            pts[step][1] = y;
            step++;
            turn = 0;
            //AT(img,x,y) = RGB565_GREEN;
        }
        else  // 前方为白色，左前方为白色（墙角）
        {
            x += dir_frontleft[dir][0];   // 遇到墙角要斜着走
            y += dir_frontleft[dir][1];
            dir = (dir + 3) % 4;   // 遇到墙角要左转一次
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
            //AT(img,x,y) = RGB565_GREEN;
        }
    }
    //丢线标志，否则由于sobel特殊性会一直往上巡线
    if(step < 20 && touch_boundary0)
        loseline0 = 1;
    //记录边线数目
    *num = step;

}
/*************************************************************************
 *  函数名称：void Right_Adaptive_Threshold();
 *  功能说明：自定义阈值二值化+迷宫算法（右边线）
 *  参数说明：获取的图像数组，输出数组（备用image0），图像宽，图像高
 *  函数返回：无
 *  修改时间：2023年5月23日
 *  备    注： 增加丢线判断
 * 前进方向定义：
 *   0
 * 3   1
 *   2
 *************************************************************************/

void Right_Adaptive_Threshold(image_t *img, int block_size, int clip_value, int x, int y, int pts[][2], int *num)
{
    zf_assert(img && img->data);
    zf_assert(num && *num >= 0);
    zf_assert(block_size > 1 && block_size % 2 == 1);
//    int half = block_size / 2;        //上交方案
    int half = 0;                     //方案二
    int step = 0, dir = 0, turn = 0;
    while (step < *num && 0 < x && x < img->width - 1 && half < y && y < img->height - half - 1 && turn < 4) {
        int local_thres = 1;
//        int local_thres;
        //自适应二值化

        for (int dy = -half; dy <= half; dy++) {
            for (int dx = -half; dx <= half; dx++) {
                local_thres += AT(img, x + dx, y + dy);
            }
        }
        local_thres /= block_size * block_size;
        local_thres -= clip_value;

        //int current_value = AT(img, x, y);
        int front_value = AT(img, x + dir_front[dir][0], y + dir_front[dir][1]);
        int frontright_value = AT(img, x + dir_frontright[dir][0], y + dir_frontright[dir][1]);
        //=======添加部分=======
        if( (x==img->width - 2  && y < img->height -20)   || x==1 || y==1 || (y==58 && step > 19)) //丢线标志，否则由于sobel特殊性会一直往上巡线
        {

            if(x==img->width - 2 /*|| x==1*/)  touch_boundary1  = 1;                //右边界是因为到最右边才停下来的，触碰到最右边，可能是环岛，十字等，
            if(y==1) touch_boundary_up1 = 1;               //走到顶边，判断坡道or障碍

            break;
        }
        //=======添加部分=======
        if (front_value < local_thres) {
            dir = (dir + 3) % 4;
            turn++;
        } else if (frontright_value < local_thres) {
            x += dir_front[dir][0];
            y += dir_front[dir][1];
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
            //AT(img,x,y) = RGB565_YELLOW;
        } else {
            x += dir_frontright[dir][0];
            y += dir_frontright[dir][1];
            dir = (dir + 1) % 4;
            pts[step][0] = x;
            pts[step][1] = y;
            step++;
            turn = 0;
            //AT(img,x,y) = RGB565_YELLOW;
        }
    }
    //丢线标志，否则由于sobel特殊性会一直往上巡线
    if(step < 20 && touch_boundary1)
        loseline1 = 1;
    //记录边线数目
    *num = step;
}

//补线 原图


void draw_line(image_t *img, int pt0[2], int pt1[2], uint8_t value)
{
    int dx = pt1[0] - pt0[0];
    int dy = pt1[1] - pt0[1];
    if (abs(dx) > abs(dy)) {
        for (int x = pt0[0]; x != pt1[0]; x += (dx > 0 ? 1 : -1)) {
            int y = pt0[1] + (x - pt0[0]) * dy / dx;// y = 左线横坐标 + x遍历差值占总差值比例 * y方向差值
                    AT(img, clip(x, 0, img->width - 1), clip(y, 0, img->height - 1)) = value; //（x，y）坐标像素（不超出边界）赋值
        }
    } else {
        for (int y = pt0[1]; y != pt1[1]; y += (dy > 0 ? 1 : -1)) {
            int x = pt0[0] + (y - pt0[1]) * dx / dy;
                    AT(img, clip(x, 0, img->width - 1), clip(y, 0, img->height - 1)) = value;
        }
    }
}

/*************************************************************************
 *  函数名称：void draw_line2();
 *  功能说明：补线 逆透视
 *  参数说明：
 *  函数返回：无
 *  修改时间：2023年6月1日
 *  备    注： 本质是等距采样
 *************************************************************************/

void draw_line2(float pt0[2], float pt1[2], float pts_out[][2], int *num, float dist)
{
    int remain = 0, len = 0;//下一次采样折线段距离
    float x0 = pt0[0];
    float y0 = pt0[1];
    float dx = pt1[0] - x0;
    float dy = pt1[1] - y0;
    float dn = invSqrt(dx*dx+dy*dy);//求平方根 求弧长积分，即输入线段前后两点距离
    //float dn = sqrt(dx*dx+dy*dy);//求平方根 求弧长积分，即输入线段前后两点距离
    dx /= dn;//此点处的cosθ
    dy /= dn;//此点处的sinθ

    //每次等距采样处理
    while(remain < dn){
        x0 += dx * remain;
        pts_out[len][0] = x0;
        y0 += dy * remain;
        pts_out[len][1] = y0;

        len++;
        dn -= remain;
        remain = dist;
    }


    *num = len;
}

/*************************************************************************
 *  函数名称：void SplicingArray();
 *  功能说明：数组拼接
 *  参数说明：输入的两个数组和容量，输出数组以及容量，x=0正向拼接，x=1反向拼接
 *  函数返回：无
 *  修改时间：2023年6月2日
 *  备    注： 用于连接补线的数组
 *************************************************************************/


void SplicingArray(float pt0[][2], int num1, float pt1[][2], int num2, float pt_out[][2], int *num, uint8 x)
{
    int i ,count;//用来计数
    for(i = 0;i<num1;i++)//复制数组1
    {
        pt_out[i][0] = pt0[i][0];
        pt_out[i][1] = pt0[i][1];
        count++;
    }

    if(x)//反向拼接
    {
        for(i = 0;i<num2;i++)
        {
            pt_out[num1+i][0] = pt1[num2-i-1][0];
            pt_out[num1+i][1] = pt1[num2-i-1][1];
            count++;
        }

    }else//正向拼接
    {
        for(i = 0;i<num2;i++)
        {
            pt_out[num1+i][0] = pt1[i][0];
            pt_out[num1+i][1] = pt1[i][1];
            count++;
        }
    }
    *num = count;
}


/*************************************************************************
 *  函数名称：void blur_points();
 *  功能说明：点集三角滤波
 *  参数说明：输入边线数组，边线总个数，输出边线数组，点集范围
 *  函数返回：无
 *  修改时间：2022年10月4日
 *  备    注：
 * 例如：kernel = 5
 *  xi = (0*xi-3 + 1*xi-2 + 2*xi-1 + 3*xi + 2*xi+1 + 1*xi+2 + 0*xi+3)/9
 *  yi 同理
 *************************************************************************/

void blur_points(float pts_in[][2], int num, float pts_out[][2], int kernel)
{
    zf_assert(kernel % 2 == 1);
    int half = kernel / 2;
    for (int i = 0; i < num; i++) {
        pts_out[i][0] = pts_out[i][1] = 0;
        for (int j = -half; j <= half; j++) {
            pts_out[i][0] += (float)pts_in[clip(i + j, 0, num - 1)][0] * (half + 1 - abs(j));
            pts_out[i][1] += (float)pts_in[clip(i + j, 0, num - 1)][1] * (half + 1 - abs(j));
        }
        pts_out[i][0] /= (2 * half + 2) * (half + 1) / 2;
        pts_out[i][1] /= (2 * half + 2) * (half + 1) / 2;
    }
}

/*************************************************************************
 *  函数名称：void resample_points（）;
 *  功能说明：点集等距采样
 *  参数说明：输入边线数组，原边线总个数，输出边线数组，获取输出边线数组个数，采样距离
 *  函数返回：无
 *  修改时间：2022年10月27日
 *  备    注：使走过的采样前折线段的距离为`dist`
 *************************************************************************/

void resample_points(float pts_in[][2], int num1, float pts_out[][2], int *num2, float dist)
{
    int remain = 0, len = 0;//下一次采样折线段距离
    for(int i=0; i<num1-1 && len < *num2; i++){
        float x0 = pts_in[i][0];
        float y0 = pts_in[i][1];
        float dx = pts_in[i+1][0] - x0;
        float dy = pts_in[i+1][1] - y0;
        float dn = invSqrt(dx*dx+dy*dy);//求平方根 求弧长积分，即输入线段前后两点距离
        //float dn = sqrt(dx*dx+dy*dy);//求平方根 求弧长积分，即输入线段前后两点距离
        dx /= dn;//此点处的cosθ
        dy /= dn;//此点处的sinθ

        //每次等距采样处理
        while(remain < dn && len < *num2){
            x0 += dx * remain;
            pts_out[len][0] = x0;
            y0 += dy * remain;
            pts_out[len][1] = y0;

            len++;
            dn -= remain;
            remain = dist;
        }
        remain -= dn;//当跨越一点采样折线距离近似直线
    }
    *num2 = len;
}

/*************************************************************************
 *  函数名称：void local_angle_points();
 *  功能说明：点集局部角度变化率
 *  参数说明：输入边线数组，边线点个数，输出角度变换率数组，两点间固定距离
 *  函数返回：无
 *  修改时间：2022年11月23日
 *  备    注：计算该点前后两点连线的斜率作为该点切线斜率的近似
 *************************************************************************/


void local_angle_points(float pts_in[][2], int num, float angle_out[], int dist)
{
    for (int i = 0; i < num; i++) {
        if (i <= 0 || i >= num - 1) {
            angle_out[i] = 0;
            continue;
        }
        float dx1 = pts_in[i][0] - pts_in[clip(i - dist, 0, num - 1)][0];
        float dy1 = pts_in[i][1] - pts_in[clip(i - dist, 0, num - 1)][1];
        float dn1 = invSqrt(dx1 * dx1 + dy1 * dy1);//此点与前一点间距离
        //float dn1 = sqrtf(dx1 * dx1 + dy1 * dy1);//此点与前一点间距离
        float dx2 = pts_in[clip(i + dist, 0, num - 1)][0] - pts_in[i][0];
        float dy2 = pts_in[clip(i + dist, 0, num - 1)][1] - pts_in[i][1];
        float dn2 = invSqrt(dx2 * dx2 + dy2 * dy2);//此点与后一点间距离
        //float dn2 = sqrtf(dx2 * dx2 + dy2 * dy2);//此点与后一点间距离
        float c1 = dx1 / dn1;//cosθ1
        float s1 = dy1 / dn1;//sinθ1
        float c2 = dx2 / dn2;//cosθ2
        float s2 = dy2 / dn2;//sinθ2
        //化简展开得到角公式(k1-k2)/(1+k1k2),即tan的差角公式==>得出两点间角度变化==>曲线斜率变化
        angle_out[i] = atan2f(c1 * s2 - c2 * s1, c2 * c1 + s2 * s1);//atan2f(y,x):返回以弧度表示的y/x反正切
    }
}

/*************************************************************************
 *  函数名称：void nms_angle();
 *  功能说明：角度变化率非极大抑制
 *  参数说明：输入角度变化率数组，角度变换数组总个数，抑制后的角度变化数组，点集奇数核
 *  函数返回：无
 *  修改时间：2022年11月25日
 *  备    注：上交方案：kernel = 21，保存极大值的数组，即一段范围内斜率变换最大的值，非极大值均设为零
 *************************************************************************/

void nms_angle(float angle_in[], int num, float angle_out[], int kernel)
{
    zf_assert(kernel % 2 == 1);
    int half = kernel / 2;
    for (int i = 0; i < num; i++) {
        angle_out[i] = angle_in[i];
        for (int j = -half; j <= half; j++) {
            //fabs():求绝对值
            if (fabs(angle_in[clip(i + j, 0, num - 1)]) > fabs(angle_out[i])) {
                angle_out[i] = 0;
                break;
            }
        }
    }
}

/*************************************************************************
 *  函数名称：void track_leftline();void track_rightline();
 *  功能说明：左右边线跟踪中线
 *  参数说明：输入等距采样边线数组，边线总个数，中线数组，两点间固定距离，一半赛道宽对应的像素个数
 *  函数返回：无
 *  修改时间：2022年11月25日
 *  备    注：
 *************************************************************************/

void track_leftline(float pts_in[][2], int num, float pts_out[][2], int approx_num, float dist)
{
    for (int i = 0; i < num; i++) {
            float dx = pts_in[clip(i + approx_num, 0, num - 1)][0] - pts_in[clip(i - approx_num, 0, num - 1)][0];
            float dy = pts_in[clip(i + approx_num, 0, num - 1)][1] - pts_in[clip(i - approx_num, 0, num - 1)][1];
            float dn = invSqrt(dx * dx + dy * dy);
            //float dn = sqrt(dx * dx + dy * dy);
            dx /= dn;//cosθ
            dy /= dn;//sinθ
            pts_out[i][0] = pts_in[i][0] + dy * dist;
            pts_out[i][1] = pts_in[i][1] - dx * dist;
        }
}

void track_rightline(float pts_in[][2], int num, float pts_out[][2], int approx_num, float dist)
{
    for (int i = 0; i < num; i++) {
           float dx = pts_in[clip(i + approx_num, 0, num - 1)][0] - pts_in[clip(i - approx_num, 0, num - 1)][0];
           float dy = pts_in[clip(i + approx_num, 0, num - 1)][1] - pts_in[clip(i - approx_num, 0, num - 1)][1];
           float dn = invSqrt(dx * dx + dy * dy);
           //float dn = sqrt(dx * dx + dy * dy);
           dx /= dn;
           dy /= dn;
           pts_out[i][0] = pts_in[i][0] - dy * dist;
           pts_out[i][1] = pts_in[i][1] + dx * dist;
       }
}

